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STUDY OP EFFECTS OF UNCERTAINTIES ON COMET 
AND ASTEROID ENCOUNTER AND CONTACT 
Gira)ANCE REQUIREMENTS 


NAS 8-27661+ 


PROJECT SUMMARY 


At the start of our work in June 1971, three 
principal tasks were assigned. Slightly restated, 
these tasks were; 

(i) Develop a deterministic algorithm for 
guidance to rendezvous with comets and asteroids 
that can handle expected large target ephemeris 
errors . 

(ii) Define the problem of determination of 
rotational state of a tumbling asteroid or cometary 
nucleus and develop possible schemes for this 
determination . 

(iii) Investigate possible contact rendezvous 
schemes including the "harpoon” technique. 

During the first year, a detailed investiga- 
tion of a rendezvous guidance technique based on 
"encounter theory” was conducted. The definition 
and formulation of the tumbling problem was made 
and several possible algorithms phrased. A first 
investigation of the harpoon problem was conducted 
and frequencies and acceleration levels identified. 

Early in the second year work, a successful 
deterministic rendezvous guidance algorithm based 
on optimal control theory was developed. The 
altorlthm was considered sufficiently important 
that, with agreement of NASA, more emphasis was 
placed on the rendezvous investigation. To 
accommodate this work, the harpoon study was set 
aside. An effort was initiated on the rendezvous 
navigation problem wherein measurements are made 
and statistically processed onboard the spacecraft 
to provide the relative state information required 


for input to the guidance algorithm. Expenditures 
on the contract were low enough so that in June 

1972 a no-cost extension of the work to September 

1973 was possible. At this time the changes In 
objective were formalized and principal tasks were 
restated to include the navigation work (and elim- 
inate the contact-rendezvous and harpoon investiga- 
tion) . 

In September 1973, delays caused by installa- 
tion of a new computing machine at the University 
prevented generation of final data. The contract 
completion date was again extended, at no cost, 
to December 15, 19T3, to allow time for this data 
generation and report preparation. 

The final report of our work is presented in 
two volumes : 

Part I. Guidance and Navigation Studies 

Part II. Tumbling Problem Studies 

Each of these volumes presents the technical 
details of the analyses conducted, the principal 
concliislons made, and listing of the coniputer 
programs einployed, including descriptions of the 
operation of the programs . Technical abstracts of 
the work are included in each volume . In Part I , 
the hody of the report reproduces a paper prepared 
for the AIAA 10th Electric Propulsion Conference 
entitled "Solar Electric Propulsion for Terminal 
Flight to Rendezvous with Comets and Asteroids.” 
(AIAA Paper No. 73-1062). The title was changed 
for inclusion in this report and a few typograph- 
ical errors were corrected. 
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STUDY OF EFFECTS OF UNCERTAIMTIES ON COMET AND 
ASTEROID ENCOUNTER AND CONTACT 
GUIDANCE REQUIREMENTS 


PART I. GUIDANCE AND NAVIGATION STUDIES 


ATaatract 

A guidance algorithm that provides precise ren- 
dezvous in the deterministic case while re<iuirltig 
only relative state information Is developed. A 
navigation scheme employing only onboard relative 
meeiBurements is biiilt around a Kalman filter set 
in measurement coordinates. The overall guidance 
and navigation procedure is evaluated in the face 
of meastirement errors by a detailed numerical 
simulation. Results indicate that onboard guidance 
and navigation for the terminal phase of rendezvous 
is. possible with reasonable limits on measurement 
errors . 

I . Introduction 

Solar electric propulsion (SEP) has been iden- 
tified as the most promising means of attaining 
the high dynamical energies that are required for 
missions to comets and asteroids. However, the 
continuous nature of SEP complicates trajectory 
dynamics and introduces difficulties in guidance 
and navigation of the spacecraft . Handling these 
difficulties will require formtilations that are 
different from those used for the iii 5 >ulsively cor- 
rected ballistic trajectories employed for plan- 
etary missions to date. Errors in SEP level and 
direction will be an important concern also because 
the errors occur over the tdiole trajectory and will 
be difficult or impossible to determine in advance 
or by direct measiu^ment during flight. 

Another problem Is that the ephemerldes of 
eisteroids, and especially comets', are not as well 
known as those for the planets . Advance knowledge 
of the position of a comet such as Encke may be in 
error by 100,000 km. or more. One reason for this 
is that comets and asteroids are not observed over 
their full orbits or in as much detail as are plan- 
etary bodies. And the orbits of comets and aster- 
oids of interest are more complex than those of the 
planets because of perturbative effects eilong 
their extended, eccentric orbits. Also, comets 
are acted upon by non-gravitational solar radia- 
tion pressure and electromagnetic force. 

The spacecraft may proceed along a nominal path 
for many months or a few years and when the target 
can be viewed, it is foxjnd that the target is not 
where it is expected to be. Assuming sufficient 
control authority to correct for this' error, there 
is then the navigation problem of determining the 
relative position and velocity of the spacecraft 


and target for the generation of terminal guidance 
commands. While transponders allow the spacecraft 
to be tracked from the earth with good accuracy, 
the eujcuracy of earth-based tracking of the target 
is much less accurate and relative state cannot be 
determined with sufficient precision by differenc- 
ing such earth -based data. It will be necessary 
to make relative measurements of some kind from 
onboard the spacecraft to insure successful ren- 
dezvous ♦ 

So. a central problem is determination of types 
of onboard measurements that can or need be made. 
But, this problem is tied to the details of the 
guidance and navigation algorithms to be used and 
the two questions must be handled together. 

Our investigations have led to a terminal 
guidance algorithm that gives accurate rendezvous 
in the deterministic case while employing a know- 
ledge of relative state only. With sufficiently 
accurate relative state estimation and control, 
rendezvous is possible without use of ground based 
measurements. And if onboard systems that fit 
accuracy, weight, power, and cost requirements are 
available , a fully autonomous guidance and naviga- 
tion system is possible. Such a system would 
eliminate signal delay for deep space targets or 
time-critical terminal maneuvers euid relieve a 
heavy work load for ground baaed tracking systems. 
Very frequent measurements would be available and 
the accuracy of attainment of terminal conditions 
improved. There is, of course, the iinportant 
question of availability of necesssiry onboard 
meaaxirement and con 5 )utation systems. Preliminary 
considerations indicate that required equipment is 
probably within the capabilities of present tech- 
nology. A more definitive answer to this question 
can be given after evaluation of possible guidance 
and navigation schemes has been meuie and specific 
system requirements identified. 

The objective of this paper is to present an 
approach to the onboard terminal gtiidance and 
navigation problem and some first results that 
indicate the approach does not require unreason- 
able onboard equipment. 

II. The Guidance Algorithm 

Simplicity is a first criteria for an onboard 
guidance scheme. And second, for comet and aster- 
oid missions , the scheme must be broadly adapt^le 
to off-nominal situations because of expected 
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ephemeris errors. It would be desirable if the 
scheme is not based on linearization about a 
nominal, but coiild proceed from any state point to 
the desired terminal conditions. 



Fig. 1. Rendezvous Geometry. 

The geometry and variables used to phrase the 
rendezvous problem are defined in Figure 1. The 
mass of most asteroids and certainly of comets is 
so small that it can be neglected in approach to 
rendezvous. Appropriate to onboard guidance, the 
equations of motion are set in coordinates 
relative to the target: 


R = F + Ol(— (1) 

S3 

where F is the SEP thrust acceleration and G is 
the universal gravitational constant. The problem 
is to determine F given the state of the space- 
craft and target^ An important simplification 
would he had if F could he specified knowing only 
the relative state of spacecraft and target . Of 
course , the dynamics of the problem are not fully 
specified by just relative state, but as will be 
found, this sin^jlifi cation is possible in a 
practical sense. The size of the sun’s gradient 
effect as expressed in the second term on the 
right of Eq. (l) is the critical factor. In 
general , this term is larger the farther the 
spacecraft; is from the target and the closer the 
target is to the sun. If we consider a target 
position of about 1 to 1.5 a.u. from the sun and 
a spacecraft position of 1.5 ^ 10 ® to 2.0 x 10 ® 
km. from the target at rendezvous , then the 
gradient force is of order 10 -^ to 10 ”® g's. 
Expected maximum SEP levels are about 10-** g’s. 

So. it is not unreasonable to ignore the gradient 
effect as a first approximation. The gradient 
effect coTJld, of course, be included as a thrust 
separately calculated from approximate knowledge 
of the target ephemeris. In either case, the 
guidance problem reduces to free space and can be 


handled by a number of available mathematical 
approaches . These ideas are not new , having been 
previously employed by Cherry for lunar orbits. (^) 

Written out in rectangular coordinates centered 
at the rendezvous point, the equations of motion 
are 





h = ( 2 ) 

= F^ + ^ [S 2 - Dg (S/D) 3 ] 

D 

X, = F, + — [S - D_ (S/D) 3] 

D 3 g3 3 3 


where the subscripts indicate con 5 >onents in the 
corresponding coordinate directions. M is the mass 
of the sun. In the spirit of approximation 
discussed above, the gravitational gradient terms 
on the right of the last three of Eqs. (2) are 
dropped. The remaining equations axe linear, 
describing just a free space motion under the 
action of controls Fq, F 2 , F 3 . With SEP thrust, 
a most meaningful criteria for choosing these con- 
trols is to minimize their time integrated square. 

+ F^^ + F^^) dt = minimum (3) 

With this starting point, the solution was de- 
termined "ty Abercrombie. (**) The method was essen- 
tially the same as employed by several others on 
related problems, and is a standard procedure from 
optimal control theory. (®) The result is 
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where the X^q, etc., are the initial conditions and 
T = Tf - T is the time to go. Note that the thrust 
components vary in a simple linear way with time. 

The guidance algorithm e 3 q>ressed in Eq. ( 4 ) was 
evaluated with, double precision digital simulation. 
Assuming a deterministic situation in which state 
is known Initially, the full equations of motion, 
Eqs. (2), were integrated for a period (the guid- 
ance interval) with controls specified by Eq. ( 4 ). 
Because of the approximations made in obtaining the 
free space equations, the true state differs from 
the free space solution. At the end of the 
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guidance interval, perfect navigation is assumed, 
the algorithm is updated vith the new, true state, 
and another period flown. This procedure is 
repeated until rendezvous is obtained or the path 
diverges. Simulations were run for typical mis- 
sions to comets Encke, D 'Arrest, and Kopff. 

Initial conditions were taken from mission studies 
made by Friedlander. (®) Update periods from con- 
tinuous to 5 days were employed. The singularity 
in the algorithm when time to go becomes zero was 
avoided by pushing time to go ahead two guidance 
intervals at the end of the trejectory. The time 
to go push was continued after rendezvous in the 
case of Encke to examine station-keeping properties 
of the algorithm. The initial conditions were 
varied over ranges of 10 degrees in direction of 
relative velocity, 10 percent in magnitude of 
relative velocity, and 100,000 km. in position to 
represent initial target ephemeris errors. 

In no case was divergence of the trajectory 
found. The treijectories were essentially straight 
line approaches to the rendezvous point. Because 
of the crude time to go push, there were overshoots 
of the rendezvous points , but accurate rendezvous 
was obtained within one or two days of the pre- 
specified time (1»0 days). In later computer r\ms , 
the overshoot has been completely eliminated by 
reduction of the guidance interval to 0,1 day and 
the time to go push to one such guidance interval. 
The spacecraft then performed small oscillations 
about the rendezvous point that increased slightly 
Eis target peri apsis was approached and decreased 
after periapsls. For Encke, with a guidance 
update of 0.5 days, an overshoot of about UO km. 
was obtained for a rendezvous standoff distance of 
100 km, in each coordinate direction. After Uo 
days from guidance initiation, slow oscillations of 
a few meters amplitude near the rendezvous point 
occiured. The amplitude Increased to somewhat 
less than 500 meters near periapsis at 100 
days and decreased to a few meters by 165 days. 
Better time to go management would improve these 
results . The thrust levels during station keeping 
were extremely low; smaller than O.5 ^ 10~® g's. 
Fig. 2 shows typical thrust histories for the 
approach phase for the three comets , It was con- 
cluded that the free space optimal control algo- 
rithm performed well enough deterministically to 
warrant investigation with simulation of a real- 
istic navigation scheme. 



III. Navigation Scheme 

A Kalman filter was chosen for forming state 
estimates from onboard measurements.. Among the 
difficulties of practical use of the Kalman filter 
are two problems that arise because of the linear 
nature of the Kalman formulation. First, linear- 
ization of the system equations leads to a model- 
ing error and it is possible that the first 
estimate of a new state based on linear propagation 
from a previoios state may not be good enough for 
the procedure to find a good statlsticeil correction 
to the first estimate. Second, the formulation 
requires a linear relation between the physical 
quantities measxired and the system state. Such 
linear relation is not the usual case and direct 
linearization of the true relations can lead to 
serious errors especially when state is not close 
to estimated values. Either of these problems can 
produce unsatisfactory performance or divergence 
of the filter. The problems have been discussed 
extensively in the literature and there are many 
ways of handling them.(^>®) We will focus on an 
approach that is simple from the computational 
point of view. 

The question of modeling error was first ex- 
amined. A filter in which the measurements were 
taken as the state variables themselves was 
programmed for digital conqiutatlon . Numerical 
simulation exhibited strong divergence. This was 
attributed to the usual fact that the filter 
rapidly reduces the state covariances and thus 
essentially ignores new measurements as they are 
made. At this point, one standard fix is to 
insert noise into the basic system equations. We 
took an even simpler approach and froze the state 
covariance matrix €ifter 5 to 10 guidance cycles. 
This technique worked extremely well. Starting 
with gross initial errors of 10 percent of the 
relative state elements (200,000 km. and 150 
m/sec.), rendezvous was attained with position 
errors of about 5 km. and velocity errors of about 
0.1 m/sec. This was accomplished with the 
previous, crude, two-step time-to-go push. We 
concluded modeling error would be manageable even 
if requiring a more sophisticated approach in a 
final formulation. 

A method of Mahra( ^ ) was chosen to handle the 
state-measurement relation problem. In his 
approach, the filtering process is carried out 
directly in what we term measurement variables. 

The system state is transformed from the rec- 
tangular coordinates used for the guidance problem 
to new variables , some of which are the measure- 
ment quantities themselves, A one-to-one relation 
between state description and measurements then 
exists. However, this approach does, as we will 
see, introduce certain other approximations into 
the statistics, but, as results show, these are 
not critical. 

The measurement variables vector is taken as 
y = (RuvKu v)T, where u and v are the direc- 
tion cosines for Xj and X 2 . The transformation 
from state variables y = g(X) is 


Fig. 2. Rendezvous thrust histories. 
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R = [(X^+Cj^) + (Xg+C^) + (X^+C^)]^ 
u = (X^+Cj^)/R 

V = (X2+C2)/R 

R = t(X^+C^)Xj^ + ^ ■*■ ^ 

u = (Xj,-Ru)/R 

V = (X_-^)/R 

? 

The Inverse transformation X = A(y) is 


(4) 


X^ = uR - 

X2 = vR - C2 

X^ = wR - 

Xj^ = Ru + ^ 

X_ = Rv + Rv 
? 

Xg = Rw + ^ 
where, w^ = 1 - - v^ 


(5) 


We then transfer to the measurement variables 
with Eq,. (4) in the form 

= g(x^) , (nonlinear) (lO) 

The best estimate of the state at "then 

given by Kalman’s relation 

^k+1 “ ^k+1 *Sc+l ^\+l '~ ^k+1^ 

where are the actual measurements, H is a 

rectangular matrix of ones and zeros that picks 
from the y^+i vector those elements that 
correspond to the actual measurements , and 
is the Kalman gain (yet to be calculated) . We 
then transform back to the state variables with 
Eg. ( 5 ) in the form 


\+l = ^^Xk+1^’ (nonlinear) 
The Kalman gain is calculated by 

\+l \+l/K ^ \+l/k ^ 


( 12 ) 


( 13 ) 


Any desirable subset of y can be chosen as the 
actual measurements;^ range R; direction cosines 
u and v; range rate R, etc. The filter process 
then proceeds as follows . Starting with a best 
state estimate Xk at time Tj^, a first estimate X"*" 
at time ts formed by a linear extrapolation 

through the state transition matrix 4> for the 
linearized system 


^ ^1 = ^ ^ 


( 6 ) 


where . 


and 


0 


“k 


«k 

0 

0 

^k 

0 
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'k-H r^ /k+1 
^k ^k 
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0 

0 

\ 
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) - 2 (-^)2] 


[(^) - (^)^3 


^ ^k+l ■ ■ T, ' ■ T 


^ [(-^) - ( 


k+1 


^k 

3 (^)2 - 2 (^) 


) ] 


(T) 


( 8 ) 


where N is a diagonal square matrix of the 
variances of the measurement errors and M|j^+i/k 
is the transferred covariance matrix of measure- 
ment variables calciilated by 


\+l/k ^ "^k \/k "^k 


( 14 ) 


where, is the state transition matrix for the 
measurement variables and is the measurement 

variables covariance matrix at Tk- (Note that no 
state dlst\irbaace has been Included) It is here • 
in the construction of that approximations are 
made, Mahra observed that 



k+1 


or. 




® 

k+1 k 


( 15 ) 


The matrices (a5,/3y) and are available from 
the best estimate of state at Tk* To form 
( 3g/3x)]j4.2, we use the first estimates at Tk+i 
obtained by linear extrapolation from Tk- The 
matrices (3il/3y) and (3g/3x) are not written out 
here. They can be foiind in Mahra *s paper. (^) All 
that remains is to propagate the covariances to 
the next time and this is done with the usual 
relation 


\+l/k+l ^+1 \+l/k 


with the time to go given by 


tsT— T T =T*-T 

F k * k+1 F k+1 


IV ■ Gui dance and N avl gat i on Evaluation Pr oce dure 

Performance evaluation of the overall guidance 
and navigation scheme requires accurate numerical 
simulation. A covariance analysis alone will not 
suffice because of the nonlinearities of the basic 
dynamics. Fig. 3 is a schematic of the procedure 
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employed. Starting at a time Tj^, an eatimate of 
the state Xjj is presumed available. For evalua- 
tion, the exact state x^ is also specified at this 
time. The estimate is put into the guidance lav 



Fig. 3. Evaluation Scheme 

to generate the SEP thrusting to be used. The 
equations of motion including the full effect of 
the sun and Keplerian motion of the target are then 
integrated accurately (fourth-order Runge-Kutta} to 
a time Tj^+i vhen measurements vill be available. 

The integrated state Xj^+i is then transformed to 
measurement variables and appropriate noise added 
to simulate eictual measurements To represent 

the onboard computations , the state x^ is propa- 
gated to time through the transition matrix 

^>J^. The nonlinear transformation gCx) to measure- 
ment variables is then made to give a first 
estimate y"*”. The Kalman gain is then calculated 
with Eqs. (l3) , (l^) , (15). The filtered estimate 
Fk+1 then made with Eq. (ll). This estimate is 
then transformed nonlinearly with f(y) to obtain 
the new state estimate And so on. The com- 

puter program for these computations has been given 
the acronym GANDER for Guidance and Navigation 
Development and Evaluation Routine. 


V. Results 

The computations for deterministic evaluation 
of the guidance algorithm were initiated at ranges 
as large as 2 x 10° km. However, such ranges are 
not possible for onboard range and range rate 
measurement with equipment that fits reasonable 
weight or power requirements. We could find no 
information that gives specific distance and 
accuracy limits for various weight and power allot- 
ments except at ranges less than 2000 km. 

Several discussions led us to believe that 50,000 
km. range and 10 measurements per day are conser- 
vative limits. At 50,000 km., optimal, trajectory 
studies (®) and our deterministic investigations 
indicate relative velocity of 20,000 km/ day is 
appropriate. The 50,000 km. range and 20,000 
km/day relative velocity correspond to a point in 
time about 5 days before rendezvous. 

Since specific accuracies coiold not be identi- 
fied, we conducted evaluations with two assumed 


measurement error sets representing accurate and 
rough meeisurements.. Angular measTirements from 
onboard science TV gives accuracies of about 20 
arc seconds. (^ »^) To allow for onboard implement- 
ation, we. chose twice this level of error for the 
angular measurements (.0002 radians or Ul,3 arc 
sec). This and other error levels used are shown 
in Table 1. 


Table 1. 

Ifeasurement error 

sets 

STANDARD DEVIATION 

SET I 

SET II 

Angular Error 

Ul.3 axe sec 

4l.3 arc sec 

Range Error 

. 002 Range 

.03 Range 

Range Rate Error 

4.63 cm/sec 

1.15 m/sec 


The remaining information necessary before 
evaluations can be made is the error in relative 
state information between spacecraft Eind target. 

It is obvious if final phase of terminal guidance 
is initiated at 50,000 km., that an ephemeris 
error of 100,000 km. (such as for Encke) cannot be 
tolerated. A preliminary study of onboard pre- 
final phase orbit determination indicates an 
improvement of relative position knowledge by as 
much QS a factor of 20 by use of onboard relative 
angular measurements only. This result was ob- 
tained on the basis that the principal error in 
target ephemeris is time of periapsis passage. 

We assumed half of the estimated improvement and 
used 10,000 km. error in each position component. 

An error of 1000 km/day (11.5 m/sec) was assiimed 
in each velocity component. The initial state 
covariance was constructed using these values as 
the standard deviations . 

In initial computer runs , a gross filter 
divergence was found as expected. The divergence 
is obvioiisly due to the modeling error introduced 
by approximations in the system dynamics . The 
covariance matrix rapidly decreased in size and 
new meas\jrements were not weighted enough. One 
method of handling this problem is to introduce 
process noise directly into the differential equa- 
tions. This has worked quite successfully 
before (^»^) but does introduce additional complex- 
ity in filter computations. A constant or adjust- 
able matrix could also be added to the covarieince 
matrix to control size of the principal elements . 

A nearly equivalent procedure was decided upon. 
After several guidance cycles , the covariance 
matrix had reduced in size considerably and we 
simply "froze" the matrix at this point. Results 
showed that a "freeze" after 10 guideince cycles or 
one day gave reasonable results. Certainly, this 
procedure is conservative. Of course, a procedure 
for better management should be developed for any 
actual system. But, our objective is a first 
evaluation and if reasonable restilts can be 
obtained with the crude freeze, then only improve- 
ment can be expected with further development - 

The navigation errors for the two data sets are 
shown in Figs. It and 5. A standard computer 
routine was used to generate noise to simulate 
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actual measurements. Different error sequences 
were used for each data type. Three different 
groups of the two runs for the accurate and rough 
data sets were investigated. There were no great 
differences in the results for the three groups. 
All groups were teurgeted at a point 100 km. in 
each coordinate from the target. 




For the accurate data Set I, the terminal 
rendezvous errors were about 5 ® km- and 50 km/day 
{ 0.58 m/sec). For the rough data Set II, the 
corresponding numbers were 280 km. and 86 O km/day 
( 10.0 m/sec). An examination of the error histories 
indicates, however, that much better results could 
be obtained with better statistical management. 
After initiation of the procedure at 5 days time to 
go, there is a rapid decrease in error from the 
initial values of 17,320 km. and 1732 km/day. It 
would seem that a better freeze time than 1 day 
could have been chosen. But, again, we were not 
interested in forcing the results. By a time of 
about one day before rendezvous , the errors settled 
down to about 15 km. and 25 km/day for the accurate 
Set I and to about 100 km. and 100 km/day for the 
roiigh Sat II. After this time there is what 
appears to be a filter divergence to the final 
values. Examination of the differences in the 


extrapolated first state values and the filtered 
estimates confirm this is the case. Ko serious 
attempt was made to correct this divergence since 
the freezing procedure would probably not be used 
in any actual Implementation. 

The SEP thrust accelerations required for the 
two data sets are given in Fig. 6 . The levels do 
not exceed values that are reasonable for proposed 
SEP systems . For the accurate Set I , the thrust is 
essentially constant over the whole period with 
deviations of only about the 5 ^ error that may be 
ej^ected from the SEP thrustors. For the rough 
SET II, the deviations are larger but not extreme. 
The initial lower value eirises because on the first 
few guidance cycles the vehicle does not know where 
it Is. The increase during the last heilf day also 



Fig. 6 . Thrust Histories 


arises becaxase of navigation error. Examination of 
the detailed trajectories shows that the vehicle is 
on track to rendezvous . The filter problem near 
rendezvous discussed in the previous paragraph is 
the cause of thrust increase. In the sample data 
form in which the guidance equations are written, 
there is no numerical singularity except at ren- 
dezvous . In the case of the rough data Set II , 
fixing the thrust at a constant value of about 
5.5 ^ 10”^ actually leads to more accurate rendez- 
vous than following the incorrect filter estimates 
during the last half day, indicating the terminal 
point control problems may be helped by reducing 
frequency of guldmice updates near the end of the 
trajectory so as to avoid small corrections in 
very small time. The nearly constant SEP level 
over most of the path also Indicates a reduced 
guidance update frequency may be possible or 
desirable there as well. 


VI . Conclusions 


The filtering procedure used was certainly not 
the best that can be envisioned. Addition of 
process noise, or a basic improvement in the ex- 
trapolation of estimated state would give major 
improvement of results. Also, the study did not 
include errors in the SEP thrust level. But 
thrust level changes due to navigation errors were 
as large or larger than the 5 ^ expected with the 
SEP thrustors. Of course, the SEP errors must be 
included in any more detailed analysis. We 
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conclude that onboard navigation is posslhle vith- 
out unreasonable accuracy requirements for onboard 
measuring equipment. But further investigation is 
clearly necessary to obtain definitive results. 

For further study we stiggest that the following 
be done: 

1. Investigate methods of reducing modeling 
error with emphasis on ease of onboard in5)lement- 
ation. 

2 . Include SEP thrust errors and a constraint 
for constant thrust level and direction rather 
than the linearly varying model now employed. 

Errors may be handled as noise or perhaps estimated 
as new state variables in the filtering process . 

3. Investigate methods of thrust level control 
not o nly at the end point, but during the whole 
terminal phase. While excessive thrust was never 
encountered in our investigation, other missions 
may have to contend with gross off-nominal condi- 
tions that can call for excessive thrust imless an 
automatic control is incorporated in the procedure. 

J4. Investigate onboard methods of target orbit 
determination that will reduce the target ephemeris 
uncertainties. 

5. Investigate the possibility of dispensing 
with some of the measurements used here with the 
objective of simplifying onboard systems. 

6. Detennine instrument capabilities, power 
requirements, weiidit , etc., for onboard measure- 
ments up to ranges of at least 50,000 km. and 
further, if possible. 


T. Schlec, P. -H. Btondish, C. J., and Toda, 
N. F. , "Divergence of the Kaiman Filter," 
AIAA Journal, Vol. 5» Wo. 6, June 196T» 

pp. iuUll20. 

,8. Jazwinski, A. H., Stochastic Processes and 
i^ltering Theory , Academic Press, New York 
ri970), PP- 301-305. 

9. Mehra, R. K., "A Comparison of Several 
Honlinear Filters for Reentry Vehicle 
Tracking," IEEE Trans, on Auto. Control, 
Vol. AC-I6, No. U, August 1971, pp. 307- 

319. 

io. Huiig, J. C. , "Autonomous Target Relative 
Navigation," Systems Research Group, Dept, 
of Electrical Engineering, University of 
Tennessee, Scl. Report S-24, January 31, 
1973. 


•References 

1. Gardner, J. A., "Solar Electric Propulsion 
System Integration Technology (SEPSIT) , 

; . Final Report,” JPL Technical Memo 33-583, 

' Vol. Ill, Nov. 1972. 

2 . Jacobson, R. A., "A Constrained Discrete 
Optimal Guidance Strategy for Low-Thrust 
Space Flight." Presented at AAS/AIAA 
Astrodynamics Conference, Vail, Colorado, 
July 16-18, 1973. 

3. Cherry, G. W. , "A Unified Explicit Tech- 
nique Performing Orbital Insertion, Soft 
Landing, and Rendezvous with a Throttle- 
able Rocket-Propelled Space Vehicle,” 

MET Instrumentation Laboratory Report 
R-lH7, MIT, Cambridge, August I963. 

4. Abercrombie, G. E., "Two Guidance Algo- 
rithms for Rendezvous Missions to Comets 
and Asteroids," Prepared under Contract 
NAS8-2766U, Auburn University, December 

1972. 

5. Bryson, A. E. , and Ho, Yu-Chi, Applied 
Opt 1 nial Control , Guin and Co , , Waltham , 
Mass . ( 1969 ) . 

6. Friedlander, A. L., private communication. 


7 



APPENDIX - COMPUTER PROGRAM GANDER 



The GANDER Program is written In FORTRAN IV and 
used with the IBM 370/155* The program is a 
research tool, not a developed production routine. 
The steps in the simulation and the names of the 
subroutines that carry out these steps are as 
follows , 


A fourth order Runge-Kutta subroutine, RUHKUT, 
is used to integrate the dynamic forces in G0FZ$ 
over sub intervals of length ET, at each time step 
(dELT) , observations are taken in OBSERV and the 
filter is used to predict the state in FILTER. The 
information generated is then put out on Unit 6 
and terminal conditions are checked in CYCLOT* 
Program sequencing and execution is controlled by 
subroutine CYCLE. Subroutine TARGET is used to 
generate the comet's position. The name, NOISE, is 
a dummy name for the functions URAND (Uniformly 
distributed random numbers) and GRAND (Gaussian 
distributed random numbers ) . Ten ( 10 ) .independent 
noise channels are shared by these two functions. 


Subroutine Names and Descriptions 


MAIN • Reads in system data and calls CYCLE. 

CYCLE ’ Controls sequence of operation and 
transfer of data between XT and SP. 

RUNKUT • Fourth Order Runge-Kutta Integrator. 

Dynamics are provided by D0FX$ and 
guidance by G0FX$. Called by CYCLE. 
Performs N integrations of step size 
DT at each call. 


* Entries : 

RKINIT called by CYCLE. 

Initialize internal variables and 
read in XT. 

DOFX$ * Compute contribution of dynamics to X. 
J is the index of the craaponants of 
XT. 

Entries ; 

E0FX$: Compute data to he used by 

all components. 

DOFX: Compute each component. 

DXINIT: Initialize internal con- 

stants and read in comet data. 

SPECIAL - Calls target for comet 
position. 

G0FX$ • Same as D0FX$ except XP is used as 
variable. May call GRAND. 

filter • User supplied algorithm to calculate 
XP. (Basic Kalman filter used in 
present listing) 

• Entries : 

FLINIT: Used to initialize arrays. 


SPECIAL; Variable ICNT is used to 
bypass covariance update section 
after preselected number of cycles. 
Calls MTNV 

OBSERV • Generates ZT; may call GRAND. 

CYCLOT • Outputs data and checks for end of 
run. 

• Entries ; 

CYCLOT: Output data 

TERMIN: ’ Check for end conditions 
satisfied. 

RECAP: If end conditions satis- 

fied; output minimum normed 
distance, velocity and associated 
times . 

CYINIT: Initialize Internal 

constants . 

TARGET • Comet's position by solution of 
Kepler’s Equation. 

MINV • Gatisslan elimination inversion 
routine . 

SPECIAL writes square (nxn) matrix as 
a vector of length n^. (Modified 
IBM SBP) 

GRAND • Generates Gaussian distributed random 
noise with given mean (RMEAN) and 
at andard de vi at i on ( STDDEV ) . 

LSCNT « Noise channel nmber. Calls URAND. 

URAND » Generates random numbers over the 

interval [0,l]. (Modified IBM SSP) . 

Block data initializes seed numbers for URAND 

Variable Names and Definitions 

XT True state vector. 

XP Predicted state vector (loaded in 

GXINIT) 

XE XT-iXP error in state 

ZT True observations 

ZP Predicted observations 

ZE ZT-ZP error in observations (RESIDUALS) 

L$L One BYTE logical array used to control 

sequencing of simulator (partially 
implemented) - 

L$E One BYTE logical array for use by error 

monitor (Eaperimantal program control, 
not implemented) . 

DT Integration stepsize (true position). 

DELT Guidance update stepsize (predicted 

position) must be an INTEGRAL multiple 
of DT. 
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N DELT/nr {an Integer (exactly)) 

TI Integration start time 

IF Integration end time 

ISLEN Number of elements In the state vector 
(XT and XP). 

lOLEN Number of elements in the observation 
vector (ZT and ZP) * 

C Rendezvous stand-off distance. 

A Target semi -major axis 

EN Target mean motion. 

EPS Target eccentricity. 

EO Target eccentric anomaly. 

TSTAR Guidance initiation time. 


Input Data 


CARD 

NO. CONTENTS 

FORMAT 

READ BY 

required system data 

1 

Title Card 

5A8 

Main 


(Ul) 



2 

Run Time Logical 

80L1 

Main 


Flags (L$E) 



3 

Run Time Error 

80L1 

Main 


Flags 



k 

N, ISLEN, lOLEN 

813 

Main 

5 

TI, TF, DT, DELT 

8F10.0 

Main 

USER DATA 

6 

XT (initial condl- 

8F10.0 

HKINIT 


tlons } 



7 

A,RN ,EPS ,E0 ,TSTAR 

8F10.0 

DXINIT 

6 

c 

8F10.0 

DXINIT 


Subroutine Inltiallzabion Entries, 


SUBROUTINES AND 

LINE NUMBERS ENTRY 

gxinit(j) 
3U, 35, 36 

37 . 38 , 39 

Initial position error, kilometers. 
Initial velocity error, km/ day. 

FLINIT 

l88 Thru 193 

Initial covariance matrix (diagonal 
elements only in present listing) . 

OVTNIT 

6l 

Standard deviation of angular 
measurement error (BANG) 

62, 63 

Fix logical if statements. Do 
not change (RMIN and RIMIN). 

OBSERV 

l6y 17, 28, 

3U, Uo. h6 

Jfean, St'd. Dev., noise channel 
(To generate rendom numbers form 
GRAND) . Ten noise channels 
available . 

27, 1*5 

Range error Std. dev. RFRAF(i) 

33, 39 

Range rate error, std. dev. RFRAF 
RFRAF(l^). 

FILTER 

iia 

Humber of guidance steps before 
covariance freeze (lO steps in 
present listing) . Remove card 
for no freeze. 
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GANDER Program Listing 


FORTRAN IV G LcVc.L Zl ^AIN 


0001 IMPLICIT REAL=*8 (A“H,0-Z) 

0002 L^10Ij:aL>^ 1 L tL»Lt£»LMONtLFtLT 

0003 00^4 M QN/V $K BL E / XT ( 6 ) r X P ( 6 ) , XE ( 6 > » Z T ( 6 1 » ZP ( 6) Vz1e ( 6 > 

0004 COiMON/TiMERZDSLTtDTtTIMEfTl , TF , N t I SLEN » I OLEN 

0003 CDiML1N/SY$TEM/L$L 140» iL»EU0» 

_ 0006 C Ojl MO ^ M S N I TR/LM JN( 20) _ 

' 00 b 7 CO M 0 N / N u'i "i c/IRAN<lOJ,DGllO j R F R 4 F ( 6 » ’ ' 

0008 C0'^M0N/JFFSE T/C(31 _ 

" 0 b ih bn e N <i i 0 n' b { 4 , 6 ) , R N ( 4 , 4» » 16(4,41' 

0010 CATA LF,LT/F,T/ 

0011 502 FORMAKdOLiJ 

0012 5U3 FGRMAT(5A3> 

0013'' 504 fnRMAT<ai3)“ 

0014 505 FGRMAT(8F10,0) 

“Q015 602 FORMAT! IHO, 30L L I 

0016 603 FORMAT! iHO, 5A8 ) 

"OCUT 604 P6KMAT!ih0 .* — InPUT CAkO LI5TM 

001c 606 FGKMAT!1H ,////» 

0019 607 FORMAT! IHO, 8 15) 

0020 6G8 Ft'RMAT! IHO, 1P6DL2.5) 

' ‘ 0021 W R I T £ 1 6 T 6 04 J 

0022 REAOI 5,503) TITLE 

0023 hKI TE!6, 603) TITLE 

00 24 READ ! 5,5Q2)LiL 

, 2 s”' ' "■■■ WR I T E ! 6 , 6“02 )"L 4 L 

Ou 26 5 , t3P2 ) L $ E _ „ 

0 J27 WRl TE ! 6, 602 ) L$E 

002o R FAUI 5,5U4)N , I SLEN, lOLEN 

0029 rtRI Th!o,6b7)N, ISLEN, lOLEN 

0030 RFA0(5,505»TI,TF,0T,DELT 

” 0031 wRl TE!b,6!)8)TI ,TF,OT,D£LT 

0032 WKlTt!6,606) 

00 33 “ . CALL CYCLE 

0034 STOP 

0035 ENG 


FORTRAN I V G LtVi: L 

21 CYCLE 

0001 

0002 


SUBROUTINE CYCLE 
IMPLICIT RFAL*6 !A-H,0-Z) 

0003 

0004 


LOGlCAL^l LiL,LSe,LMQN,LF,LT 

CGMM0N/ViR8LE/XT!6) ,XP!6J ,XE(6),ZT(6) ,ZP(6),ZE<6) 

0005 

0006 


CUMMGN/T$MER/DELy,DT , TI ME , tl , TF , N , I SL EN, I OL^ 
Cb'(MuN/SYiTcM/LSL(40) fLte! 10) 

0007 

OOOB 


COMMlN/M$NITR/LMQN( 20) 

COMM JN/,N0I$E/IRAN(10) ,DG(10) ,RFRAF(6) 

0 009 
0010 


CCMMON/OFFSET/C! 3) 
DATA LF,LT/F,T/ 

OOil 

c 

Cn ENSIGN B( 4, 6) ,RN(4,4) , T6(4,4) 
INITIALIZE SUBROUTINES 

0012 

0013 


CALL RKINIT 
CUM = 0XINIT! i ) 

0014 

0015 


CU'( = GXIN1T! 1) 
DUM=UFINIT! 1 ) 

JO Ib 
0017 


CALL CYINIT 
CALL FLINIT 

0018 

C 

CALL GVINIT 

COMPUTE TRAJECTORIES 

0019 

0020 

1 

CALL PUNKUT 

IF! LFL! 2) )GG TO 2 
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FORTRAN IV G ttVEL 


TARGET 


0001 

0002 

0003 

0004 

0006 

0007 

0008 
0009 



SUBROUTINE TARGETIS,T) 

implicit realms (A-HtO-ZI 

OHENSICIN S<3» 

COMMDN/VARl/A,RN,EPSfET,TSTAR»DET 






ET=ET+DcT 
DO i. I = i»100 


SINIET=DSIN(ETI 


C0SET=0C0S< ETI 


F=«N>MT+TSTARI-ET + EPS»SIN 
DF=tPS*COSET-1.000 


6T=cT-F/0F 

CIF=DABSIF/DF) 

IF( OIF.LT.1.00-10)GQ to 2 
CUMTINue 

MRlTE<6.6dUDIF 

DET=£T-£0 


SUI = A*(COSET -EPS) 

S(2 )=A#DS0RT( 1.000-EPS<‘EPS)*SINET 

S(3)=0,OD0 

RETURN 

END 


r 
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fortran 

IV G LEVEL 

21 RUNKUT 

\ 

0001 


SUBROUTINE RUNKUT 

0002 


implicit REAL*8 (A-H,0-ZJ 

0003 


LOGICAL*! L$L.L*E,LM0N«LF#LT 

0004 


LOGICAL*! LS1,LS2 



C0'iM0N/V$R6“LE/XT(6) *XP<61 rxlTbl , ZT 16 ) »ZP (61 |ZEI6I 



C0MM0N/T$MER/DELT,0T,TIME,T1*TF.N,ISLEN,10LEN 

0007 


C0^M0N/SY$TEM/LtL(40)«LSE(10) 

OOOB 


C0«1M0N/M$N1 TR/LMON< 20) 

0009 


CQMMON/N0ISE/IRAN(10)tDG(10) tRFRAF<6> 

OUlO 


C0'1MDN/QFFSET/C<3) 

OOU 


CliENSION jTlNKb) ,^UM<6i ^ 

0012 


DATA LF,LT/F,T/ 

0013 


L$L t 12)=LT 

0014 


DO 1 ICyCLE=l,N 

0013 


CO 33 I=IfISLEN 

0016 

33 

SUMII)=0.0D0 

0017 



0018 


CO 10 n=it4 

0019 


LS1<I1.EQ.2. 0R.I1.EQ.3 

0020 


LS2=Il.E0.4 

0021 


LtLUl» = Il.E0.3 



F=F1 

0023 




0024 


IF( LS1)F=F2 

0025 


IF( LS1IFS=F3 

0026 


IFt LS2)FS=F4 

0027 


TS=TIME*0T*FS 

0028 


DO 20 i=l,ISLEN 

6IFI9 

20 


0030 


DC 31 1=1, NPl 

0031 


J = I-l 

00j>2 


IF( J.GT«0» go to 2 



CUM=D0FX$I J,TS) 

0034 


CU'1=G0FXt(J,TS) 



T$L < 12)=LF 

0036 


GO TO 31 ' 

0037 

2 

XINT< J J=0T*I DOFXI J)4-G0FX{J)) 

0038 


SUM( J » = SUMIJ)+F4XINTIJ> 

0039 

31 

CONTINUE 

0040 


LtL( 10)=LF 

0041 

10 

CONTINUE : 

0042 


TIME = TIME<-DT 

0043 


bo 11 I=l,ISLEN 

0044 

11 

XTI 1 )^XT( I)*SUM1 I ) 

0045 

1 

CONTINUE 

0046 


RETURN 

3047 


ENTRY RKINIT 

0048 


REAOl 5,501) (XT(I l,I=l,ISLEN) 



WRT fET6766 1 ) i XT ( I) , I = 1 , I SLEN ) “ 

0030 

501 

F0RMATI8F10.0) 

0031 

601 

FORMATUHO,1P8012.5) 

0052 


Fl=i,ODO/6.0DO 

0053 


F2=2.0DO*F1 * “ 

0054 


F3=l. 000/2. ODO 

0055 


F4=l.0D0 

0056 


F5=0.000 



THE=TI 

00 58 


NP1=ISL6N+1 


FORTRAN IV G LEVEL 21 RUNKUT 


00&9 DO 32 1=1.ISLEN 

0060 32 XIMTCI ) = XT( I t 

0061 
0062 


RETURN 

END 
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FORTRAN IV 

G level 

21 OQFXi 

0001 

0002 


FUNCTION D0FX$(J»TSJ 
IMPLICIT P£AL*8(A-H,0-2I 

0003 

0004 


LOGICAL*! L$Lf LtE,LHONtLF,LT 

C0MM0N/V$RBLE/XT(6) tXP(61 f XE16) tZT(6l f ZP( 6) >ZE (6) 

0005 

0006 


COHMQN/T$MER/DELT,DT,TIMt,tl,TF,Nt ISLENtlOLEN 
C0MMON/SYSTEM/LtL(40» #LtEI 10) 

0007 

oooa 


C0MM0N/M$NITR/LM0N<20) 

CON MON/NO I $E/ IRANI 10) « OGI lO) ,RFR AF 1 6 ) 

0009 

0010 


COMMON/VARl/Af RN»EPStEO»TSTAR,OET 
COMMON/OFFSET/CI 31 

jraiT 

0012 


DIMENSION D(3I,S(3) 
DATA LF,LT/F,T/ 

0013 

0014 

99601 

FORMATIIHO, 14, • IMPROPER INDEX *DOFX* ) 
IF! .NOT.LtLI iniCALL TARGETIS,TS» 

; 0015 

! 0016 


S2=0.0D0 

D2=0.0D0 

<JUT7 

ooie 


DD 1 1=1,3 ' 

DU )=s(iH>cm*xTm 

0019 

0020 

1 

D2=D2+0( I )*»2 
S2=S2+Si I )**2 



DN=DS0RTID2I 
SN=DSQRTt S2) 

0023 

0024 


RAT I=GM/I SN*SN*SN) ^ 

RAT2=( SN/DN)**3 

0025 

0026 


DOF X $=0,000 

IFI TF-TIME.GT.DELT)RETURN 

0027 

002B 


IFI .NJT.L$L< lOlIRtTURN 
WRITE(6,602JTIME,XT 

0030 

bUT 

TDSMATllHO, *tND STATE * ,2X,M lMb = * ,F10.3/1M ,lPi»Dl2.5» 

RETURN 

0031 

0032 


ENTRY OOFX(J) 

GC TU ( 99999,99999,99999,99998,99998,99998) ,J 

0033 

0034 


WRITE! 6,99601) J 
DaFX=O.ODO 

0035 

0036 


LSE (2)=LT 
RETURN 

0037 

003b 

99999 

CCFX=XT(J+3) 

RETURN 

0039 

0040 

99990 

OOFX=PATI*( SIJ“3)-D( J“3)*RAT2) 
RETURN 

5041 

0042 


ENTRY OXINITIJ) ““ 

READ! 5,501)A,RN,EPS,E0,TSTAR 

0043 

0044 


EO=RN*TSTAR ' ' ' 

WRI TEI6,601)A,RN,EPS,EO,TSTAR 

0045 

0046 

601 

F0-?MAT!8F10.0) - - 

FORMAT! IHO, IP80L2.5) 

0047 

0048 


■READ! 5,501JC 

WRI TE(6,601)C 

0049 

0050 


GM= 9.90549020 ' 

DET=l,0Q~3 

0051 

0052 


DXlNlT=O.ObO ‘ ■" 

RETURN 
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FORTRAN 

IV G LEVEL 

21 G0FX$ 

0001 

0002 


FU'JCTION GOFXS(JtTS) 

IMPLICIT REAL*8( A-H,0-Z» 

0003 

0004 


LOGICAL*! L$L»LSEfLMON,LF*LT 

C0^M0N/ViRBLE/XT(6l .XPI 6) «XE < 6) « ZT( 6 ) t ZP (6> • ZE C6 ) 

— 

0006 


«MW0KI/TiMeR/DELT,DT,TIME,Ti;TP,»4,IiLCf^,!6LEN 
COHMON/SYtTEH/L$U40) ,L$EaO) 

0007 

oooe 


CO^MON/M(N1TR/LMON(20I 
COHMON/NGKE/ IRANI 10) vOG( 10) *RFRAE(6) 

0009 

0010 


COMMON/FORCE/Ff 3) 
COMMON/OFFSET/C( 3) 

MU 

0012 

99601 

CAta lfVlT/f,t/ 

FORMAT) lH0,l4f 'IMPROPER INDEX *GOFXM 

0013 

0014 


IF( L$L<12) )TIM1=TS 
TAUO=TF-TIMl 

0015 

0016 


TAU=TF-TS 

TRAT1=(6*000/TAUO**2)*< l-OD0-2-0OO*)TAU/TAU0) ) 

— 

0018 



0020 


RETURN 

ENTRY GOFX(J) 

0022 


GO TO ( 99999,99999, 99999f99998t99998t 99998) fJ 
WRITEI6f 99601) J 

5U75 

002^ 


UE(3J=LT 

GOFX=O.ODO 

' ~M^5“ 

0026 

99999 

RETURN 

GOFX°O.ODO 

0027 

0028 

99998 

RETURN 

F)J-3l=TRATl*XP) J-3)>TRAT2*XP)J) 

002^ — 

0030 


G0FX=FfJ-3) 

RETURN 

0031 

0032 


ENTRY GXINITIJI 
DO 90001 I=l,ISLEN 

0033 

0034 

90001 

xp( n=xt( 1 ) ^ 

XP) 1)=XP)1J>10000. 

— 

00 36 


XPI 2I=XP)2I*10000. 
XP) 3)«XP)3)^10000. 

"75oT? — 

0038 


XP( 4)=XP)4)*1000. 
XP) 5)=XP(5H'l000. 

0039 

0040 


XP( 6)=XP(6)^1000. 
GXINIT=0.0D0 

ffDTI — 

0042 


RETURN 

END 
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FORTRAN 

IV G Lev EL 

21 FILTER 


0001 

0002 


SU3R3UTINi FILTER(B,RN,T6» 
IMPLICIT REAL*8I A-H.O-Z) 


0003 

0004 


LOGICAL^l L$L»Lt£»LMON,LF,LT 

COM MON/ V$RBLE/XTI 6), XP( 61, XE< 6) • ZT(6) ,ZP (6) »ZE {6 1 


0005 

0006 


CO'IMaN/TSMER/DELTfDTfTINEtTI ,TF,N,ISLEN»IOLEN 
C0*1 MON/S Y$TEM/LtH 401, L$E( 10) 


0007 

0008 


C0'1M0N/rtSNITR/LM0NC20) 
COMMUN/NQItE/IRANI lOt ,DG( 10» ,RFRAF(6) 


0009 

0010 


CQ*1M0N/KALMAN/RM<6,6),FILT(6,4I ,U(61 
CUSMUN/3FFS£T/C<31 

' 

OOii 


OmcNSION T1(6I,YL(6> ,Y(6),T2(6,6»,T3C6,6I fT4l6,6) 
IB(I0LEN,6),RN< lOL EN , I GLEN ) , TSf 6, 6 ) ,T6 UOLENt 1 QLEN) 

,QP (6,6), 
,PHI(6,6) 

0012 

0013 


DIMENSION LL(6I,MH(6J 
DATA LF,LT/F,T/ 


0014 

0015 

80 

WRI TE{6, 60U0LEN 

FL3 MAT ( 20X, • IOLEN=» ,121 


3oT6 

0017 


ICNT=ICNT+1 
DO 38 I=1,I0LEN 


0018 

0019 

32 

CO 32 J=l, lOLEN 
RN( I, J 1=0.000 



38 

DO 38 J=1,ISLEN 
BU , J )=O.OCO 


0022 

0023 


IF( I JLEN.E0.2IG0 TO 34 

1F( LtU20).ANO.IOLtN.EQ.3IGO TO 37 


0024 

0025 

33 

CO >3 1=1,3 
RU , U = l,000 


0026 

0027 


IF( IOLEN.cU. 4)6(4,61=1.000 
GO TO 36 


ooH 

0029 

37 

BU ,2)=1.0D0 
R(2,3)=1.000 




B(3,6)=1.000 
DO 39 1=1,3 


0032 

0033 

39 

RM I , I J=RFRAF( I+l)**2 
GO TO 35 


00 34 
00 35 

34 

“ai! 1,2) = 1.000 
B(2,3)=i.000 


0036 

0037 


PN( 1, H=RFRAF( 2)**2 
KNI 2,2»=RFRAF(3)**2 


0038 

0039 

36 

GO TO 35 

00 30 I=1,I0LEN 


0040 

0041 

30 

35 

KN( I, n=RFRAF( I)**2 
TAUK1=TF-TIME 


0042 

0043 


tAJR=tAUKl+DELT 

RAT=TAUK.1/TAUK 


0044 

0045 


RAT2=RATVrAT 

RAT3=RAT2*RAT 


0046 

0047 


giTic wiT»T>T j .»■* 1 iTiia :fii 


5048 

0049 


F=TAUK1*DIF 

0=3 .0D0*RAT2-2.0n0*RAT 


00 50 . 
0051 


E=-6.0D0*DIF/TAUK 
00 3i 1=1,3 


0053 


PHI ( I , I )=A ' 

PHI ( I , I+3)=F 


3^54 

0055 


PHI U+3. 1 )=E 
PHI U+3, l + 3)=0 


0056 

31 CUM T I NOE 
C PREDICT X STATE 
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OC57 

v)056 


DQ 10 I=1,ISLEN 

Tl(I)=O.ODO 

0059 

0060 


CO iO J=1,ISLEN 
10 Tl(H=THIJ^PHm,JI*XF(J» 

0061 

C 

TRANSFORM TO Y SYSTEM 

YU l=DSORT{ (Tl(l)4-C(l» ( TH 2 1 ♦C 1 2 > 1 **2+< TU 3 )^C( 3 M **2l 

0062 

0063 


Y(2 >=< Tl(l)+C( l))/Y(l» 
Y(3 ) = (Tl(2l>C(2n/Y< U 

0064 

00o5 


W=( TU3)+CI3)l/ym 

YI9 J = C<T1U M-C(in*TlC4H^<TU2)+C(2» » ♦!! < 5 »♦ C TU 3 )♦€ 1 31 » ♦Til 6 ) I 

0066 


♦/Yin 

yi5)=ITll4I-YC4»^YI2n/Yll) 

0067 

0068 


YI6»=(T1(5I-Y(4)*YI3)»/YI1) 
WD= (Tll6l^YI 4)^W|/YI 1) 

0069 

C 

COMPUTE LEFT PARTIAL DERIVATIVE 
T2(1»1)=Y12I 

0070 

0071 


T2I 1,2>=YI 3) 
T2I it3) = W 

0072 

0073 


T2( 2»ll = ( 1.0D0-Y(2)**2) /Yl 1) 
T2I 2,2)=-Y{ 2 l*YI 3) /Yd) 

0074 

0075 


T2I 2,3)=-Y(2)*W/Yd) 
T2I 3,l)=T2(2.2) 

0076 

0077 


T2I 3,2) = { l.0D0-Y(3)**2)/Yd) 
T2I 3,3)=-(Y| 3)^W)/Yd) 

0076 

0079 


DO il I=l»3 
00 ll J=l«3 

0080 

OO&l 


11 T2( I+3,J+3) =T2I I , J) 
T2( 4,n=Y( 5) 

0062 

0083 


T2( 4»2)=Y(6) 
T2I 4,3) = WD 

0084 

0065 


T2I 5»lJ=-(2.000*YI2)^Y(5) + li.ODO-YI2)^^2l*YI4)/YU))/Y!l) 
T2(5,2)=-(Y(3)*Y(5) + Y|2)^Y<6)-YI2nYI3)*YI4)/Ymi/Ym 

008o 

0067 


T2I 5,3)=-(rt*Yl 5)+y(2)^WO-Y(2)*W^Yl4)/Y(l) )/Y(l) 
T2( 6d)=T2( 5,2) 

3088 

0089 


T2I 6,2)=-( 2.000^YI3)^YI6l+ll-000-Y(3)^»2)*YI4)/Yll) )/Ydl 
T2I 6,3)=-(W*YI6)>YI3)^H0-YI3)»W^y(4)/Y|l) )/Y(l) 

0090 

C 

COMPUTE RIGHT PARTIAL DERIVATIVE 
T3( 1,1)=YL( 2) 

0091 

0092 


T3( 4,4)=YLI 2) 
T3I 1,2)=YL( 1 ) 

0093 

0094 


T5( 4,5)=YL( 1 ) 
T3( 2, 1) = YL( 3 ) 

0095 

0096 


T3( 5,4)=YL( 3 ) 
T3(2,3)=YL(i) 

0097 

0098 


T3( 5,6)=YL( 1) 
T3I 3, 1 )=k«L 

0U99 

0100 


T3< 6,4)=WL 

T3I 3,2)=“YL(2)^YL( l)/WL 

0101 

0102 


T3I 6, 5)=T3( 3,2) 

T3( 3,3)=“YL( 3I^YLd)/WL 

0103 

0104 


T3I 6,6)=T3I3,3) 
T3I6,1)=WDL 

0105 

0106 


T3I 6,2)=-(XP(4)/WLn-YL( 1 ) ♦YU 2 ) ♦ WDL/WL^^Z 
T3I 6,3)=-UPI5)/WL)+YL( l)^YLI3)^WDL/WL^^2 

0107 

c 

COMPUTE PSI 

DO 12 I=l,ISLEN 



DO 12 J=1,ISLEN 
T4I I , J)=0.0D0 




l6 
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0110 

0111 


DO 12 K=1,ISL6N 
CO 12 L=1,ISLEN 


0112 

C 

12 T4( I»J>*T4U,J»+T2(I,K)#PHItKtL>*T3(L»JI 

PPELICT COVARIANCE 




DQ-fi l = l,ISLEN 
00 13 J=l,ISL6N 


0ll5 

0U6 


T5( I,J|=OP| I ,J| 
L’C 13 K=1,I5LEN 


0117 

0113 


DO 13 L=l,ISLEN 

13 T5( I ,J J=T5( I »J ) + T4( I t K 1 (K , L 1 *T4lJ , L > 


0119 

c 

CGMPoTFTHTTTrfTR ^ 

DO 14 I=1,10LEN 


012J 

0121 


DO i-t J = 1,I0LEN 
T6( I,JI=RN( I ,J) 


0122 

0123 


no 14 K=l, ISLEN " ■ 

CO 14 L=l, ISLEN 


0125 


14nT6( ltJI = T6«IfJI+BJItKl*T5tk»L)*6<JfU 
CALL MInV(T6,I0LEN. 16,LL»MM,D> 


0126 

0127 


DO 15 I=i, ISLEN 
DC 15 J=l,iaLEN 


0123 

0129 


FILTI I,J)=0*ODO ' 

Du 15 K=l, ISLEN 


UT3I3 

0131 


UO lb L = l,10ltN 

15 FILni,J)=FILT( I,J)+T5( I ,K)*6(L,KI*T6(L» J) 


0132 

c 

CliMPjTE prFoiCtRT observations 

DO i6 I=i,IOLEN 


0133 

013^ 


zpi n=o,oDO 
DC 16 J=1,ISLEN 


crrn; 

c 

— 6"7P'I n=zp{u>fin,j)*v(jj 

COMPUTE THE ERROR IN OBSERVATIONS 


0136 

0137 


00 17 I=1,IGLEN 
17 ZEI I ) = ZT( I I-ZP( I ) 


0138 

c 

UPDATE Y 

DC 13 1=1, ISLEN 


UTT5 

0140 


Tu n=Y( 1 j 

DO 13 J=1,IDLEN 


014L 

c 

18 Tl(n = Tl{n+FILT(I,J»*ZE(J) 

UPDATt COVARIANCE 


0142 

0143 


1F{ ICNT.GT.10.AND*ICNT,LT.32IGO TO 99 

IF( ICNT.GT.40I go TO 99 


0145 


Dm 9 1 = 1, ISLEN “““ 

DC 19 J=l, ISLEN 


0146 

0147 


T4(I,jf^.odo ' ■■ 

DU 19 K=1,IQL£N 


0149 


19 " T4( I7J 1 =T4 (TTJT+F I LTTr. K) *aTK, J i 

DO 20 1=1, ISLEN 


jrr5^5 

0151 


CO 20 J = l,ISLcN 

T4( I ,J )=-T4t I, J» 


0152 

0153 


iF( r.Eg.j»T4ri ,j»=T4n , jn-i.odo 

20 CCMTINUE 


0154 

0155 


CO 2i I = l,ISLEN ■ ■” ■" 

Du 21 J=i, ISLEN 






0156 

0159 




0160 

c 

SAVE Y(K + l,K+n 
DC 22 1=1, ISLEN 
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0161 

C 

22 VL( 1 J=Tll I) 
CONVERT TO X 

0162 

0163 

XP( 1)=YL(2)*YL( U-C( 1) 
XF( 2)=YL< 3I*YLI U-C(2» 

0X6^ 

0165 

XPI 9)=VL( 1)#YLI 5)+YL(9)*YL(2» 
XP( 5»=VL(i)*YL<6J^YL(9)*YL(3J 

0166 

0167 

XP( 3)=050RT( YL ( U**2-( XP( l)+C( U ) ♦*2“ ( XP ( 2 ) +C ( 2 ) 

XP( oJ = IYLU)*YL(9)-( XPd UCm)*XP(9)-4 XP(2)*C( 2U’<'XP(5I )/(XP(3U 

0168 

6C(3 ) ) 

wL = ( XP( 3}+C( 31 )/YH 1) 

0169 

0170 

H0L=(XP(t)|-YL(9)*WL»/YL(l) 
WPIT6(6t 912MXPU) ,J = 1,6» 

0171 

0172 

912 FQ&^AT(6D12.5//J 

RETURN 

0173 

C 

ENTRY FLINIT 
ZERO ARRAYS 

0T74 

0175 

Qi. 101 I = 1,ISLEN 
CG i02 J=l, I SLEN 

0176 

0177 

T2( It J)=0,0D0 
T3( It JJ=O.ODO 

0178 

0179 

RIM ItJ)=0*000 
PHI ( I,JI=0.0D0 

0160 

Olei 

102 0P( ItJJ=O.ODO 

nr 10 i j=itioLEN 

0182 

C 

101 FILTI ItJ)=0,0Q0 
INITIALIZE VARIABLES 

0183 

0189 

YL( 1) =OSOkT( iXP( H+CU» ^♦*2+(XP(2)^^C< 2J )Y*2+< XP(3) + Ci3n**2» 
YL( 2) = (XP{U+C(in/YL(1) 

0185 

0186 

YL( 3) = UF(2I+C( 2) >/YL( 11 
WL=(XP(3I+C{3)i/YL(U 

0187 

YL( 9I = < (XPI 1 )+C( 11 )*XP(9)+<XP42"H-Cr2n*XP(5) + (XP(3)-»'C(3I I*XP(6H 

♦/yl ( n 

0188 

0169 

YL(5)=(XP(9>-YL(9I*YL( 2 ) ) /Vl ( IF 
YU 6) = (XP( 5J-YL(4»>^YL( 3) ) /YL ( U 

0190 

0191 

»M( 1, 1» = 1.0D8 

kM( 2,2) = 9.00-02«< I'YU 2I**2» 

0192 

0193 

RM( 3,3I=9«00-02*( l-YL( 3)**2» 
RMI 9t4)=1.0G6 

0199 

Oi95 

RM( 5t5>=1.00-05x=YL< 2)**Z 
R,Mt 6,6)=1.00-0 5^YL(3K*2 

0196 

0197 

W0L = (XP(6)-YU9)«WL»/YL(U 
ICNT=0 

019b 

0199 

RETURN 

END 
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21 OBSERV 

0001 

0002 


SUBROUTINE OBSERV 
implicit REAL*8I A-H,Q-Z) 

OOOi 

OOOA 


LOGICAL*! L*L,Lte,LMON,LF,LT 

CGiM0N/VSRBLE/XT(6) ,XP(6) tXE(6),ZTI6) fZP(6) fZEI6J 

0006 


CO'4MGN/TTMcR/OELT,t>t,T!ME,Tl ISLENt IOL£^^ 

C0'<M0N/SY$TEM/LtL(40» tL$E(10) 

0007 

OOOB 


CG'4MQN/M$NITR/LHQN(20) 

COM MON/NO UE/ 1 RANI lOl.OGUOl fRFRAFI6) 

0009 

0010 


COMMON/OFFSET/CI 3) 
UATA LF,LT/F,T/ 

c 

c 

□bSfcRVATIONS 

RFRAFd TO 4) = STODEV FOR ZTU TO 41 RESPECTIVELY 

0011 

0012 


RA\iGE=OSORT| (XT( 1 ) 4-C 1 1 M **2* I XT 1 2 KC ( 2 H XT ( 3KCI 3»l **2 > 

ZTI 2l = (XT(lJ*Can/PANGE 

0013 

0014 


ZT( 3)=(XT(2dC(2> I/RANGE 
UAC=DARC0SI ZT( 2) » 

0015 

0016 


“Tac=darCoSIzT(3M “““ 

OGI 2) = GRAN0( 0. 000« BANG* 2) 

0017 

0013 


CGI 3)=GRAN0( 0.000,EANGt3) 
UAC=UAC+DG( 2» 

0019 

0020 


VAC=VAC*bG( 3) 
ZT( 2)=0C’JS(UAC) 

0021 

0022 


ZT( 3I=0CQS<VAC> 

RFRAFI 2I=OSORTIEANG**2*(1.0DO-ZT(2»**2)I 

0023 

0024 


RFR AF( 3l = DS0RTirSNG**2*U.0D0-ZT(3»**2n 
IF<LSL(20)I GO TO 10 

0025 

0026 


IFI RANGE. GT.RMINIGO TO 2 
ZT< 1I=RANG£ 

0027 

0028 


Rf-RAf^( ll=DAd5Ii7 1 1 1*3.00-021 

CGI i)=GRAND(0.000,RFRAFI U,1I 

0029 

0030 


ZTI i)=ZT< i)+OG( U 

IF( RANGE. LT.RMIN. AND. range. GT.ROMINJGO TO 3 

00 31 
0032 


IGLEM=4 

ZT(4J = ((Xni)*CUn*XT(4H-UT(2dC<2) »*XT 1 5) ♦ ( XT 13 I^C ( 3 ) )*XT(6n 

0033 


*Z/T( 1 ) 

RFRAFI4l=i.0D02 

0034 

0035 


OGI 4)=GRANO(O.ODO»RFRAF(4)*4) 
ZTI 4)=ZTI4>+0G(4| 

0036 

0037 

10 

GO TO 5 

IF! RANGE. GT.ROMIN) GO TO 2 

5038 


0039 

0040 


RFRAFI4I=1.0002 ' 

OGI 4l=GHANDI0.0D0tRFRAFI4)«4) 

0041 

0042 


ZTI 4I=ZTI 4dDG(41 “ 

IF! RANGE. LT.ROMIN. AND. RANGE. GT.RMINI GO TO 6 

0043 

0044 


ICL EN=4 
ZTI H=RANGE 

0045 

0046 


~RFTAF I i ) =D A dS(ZTIll*3. OD- 02 ) 
□G( ll=GRANOI O.OPO, RFRAFI 11 ,11 

0047 

0046 


ZTI U=2T(1)+0GI n 
GU TO 5 

0049 
00 50 

2 

WRI TE(6, 11 (dgilsCnT I ,lSCnT=2,3I 

I0LeN=2 

0051 

0052 

3 

GO TU 4 

IGLEN=3 

0053 

0054 

5 

1 

^jRI TEI 6, 1) IDGTLSCNT)»LSCNT=1, lOLENI 

FOR MAT ( /// ♦ lOX , • NQI S6* , 5K , IP4D12. 5) 
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0055 GO TO 4 

00 5o 6 HP I TE( 6, l)lpG(LSCNT) ,LSCNT=2f4> 

0057 

0058 

0J6Q 
0061 
0062 
0061 

0064 

0065 


FURTRAN IV G LEVEL 21 GRAND 


0001 FUNCTION GR AND I R MEAN , STDDE V f I SLC T 1 

0002 L^PLICJT REAL*8(A-H,Q“Z) 

C PURPOSE 

C COMPUTES A NORMALLY DISTRIBUTED RANDOM NUMBER WITH A GIVEN 

C mean and standard DEVIATION 
A=0.0D0 
CO 50 1=1,12 
50 A = A«-JRANO( ISLCT) 


0006 
0007 
000 B 


GRAND=( A-6.0D0>*STDDEV+RMEAN 
RETURN 

Tnd 


OOOi 

0004 

0005 


IULEN=3 

4 RETURN 

ENTf^ OVINIT 

A=O.ODO 

EANG=,0002 
RMIN=1.0 D2Q 
RDM IN=i;0D20 
RETURN 

— HsTD 


FORTRAN 

IV G LcVEL 

21 UKA.ND 


0001 

0002 


FUNCTION UKAND( ISLCT) 
IMPLICIT REAL*8IA-H,G-Z) 


0003 

0004 


COMM IN/NO I $E/ IRANI 10) ,DGI 10) ,RFRAFI6> 
IY= 1KAN( ISLCT)t65539 


0005 

0006 

5 

IY)5,6,6 

1Y= IY + 2l474E3b47+l 


0007 

OOOB 

b 

URAND=DFLUAT( IY)*4,656613D-10 
IRANI ISLCT» = IY 


0009 

0010 


RETURN 

ENTRY URIMTUSLCT) 


0011 

0012 


urand=o*odo ^ 

urinit=o.odo 


0013 

0014 


RETURN 

EtND 



FORTRAN IV G LEVEL 21 


6LK DATA 


0001 

0002 

J003 

0004 


GUOS 


ELJCK DATA " 

IMPLICIT REaL*8 (A-H,U-Z) 

CUMMUN/NUI t£/IRAN( lOJ ,0G( 10) ,RFR4F(6) ' " 

CAT A I R. AN/69 £00661, 542 18059,510 70625, 1523 9339. 7S8<???^7. 
«I0!» 13327, 81767867,59847 621,52031357,2 6256073/ 

_LND 
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21 CYCLOT 

0001 

OOQi 


SU6RQUTINE CYCLOT 
IMPLICIT REAL*8(A“H,0-Z> 

0003 

0004 


LUGICAL+1 LSLiL$E,LMON,LFfLT 

Cu'^M0N/V$RDLE/XT|6I tXP{ 6) »XE( 6) ,ZT(6) ,ZP C6» ,ZE<6) 

0006 


COMMUN/TtMEK/nELT»DT,TIM£,TI ,TF,N,ISLeN,10LEN 
COMMON/SY$TtM/L$L(40l fL>6( 10) 

0007 

0008 


C0'^MUN/M$N1TR/LM0N(20) 
CU'4MCN/NOI$E/IRAN(iO) » DG (1 0) ,RF R AF ( 6 ) 

0009 

0010 


CtHMUN/KALMAN/ P(6,6I,FILT(6»4),U(6I 
C0'1M0N/FDRCE/F( 3) 

rm 

0012 


C0'1MGN/DFFSET/C( 3) 
DATA LF,LT/F,T/ 

0013 

0014 

601 

603 

FORMAT! IH ,3X»*TRUE STATE VEC TOR • /4X » IP6D 12 . 5 ) 
FURMAKIH ,*TIME = * , 1PD12.5) 

0015 

604 

FORMATdH »*NORMED 0 1 STANCE= * t IPOl 2. 5 , 3X , * NORMEO VEL0CITY=*. 
11PD12.5,3X, 'NORMED F0RCE=* » 1PD12. 5) 

0017 

605 

606 

FORMAT! IHO, • t$$ R EN&!:i i! V0U5 t$SM 

FORMAT! IHOt * MINIMUM NOPMED D I STANC 5= ' , IPDl 2. 5 . 3X ♦ < AT TIMERS 

0018 

607 

1 1PU12.5) 

FORMATdHOi 'MINIMUM NORMED VELOCITY = * ♦ lP012 . 5 » 3 X , * AT TIME=** 

0019 

608 

1 1^012.5) 
FORMAT!////) 

0020 

OOZi. 

609 

610 

FORMAT! 1hO» OUT OF TIME M 

FGRMATdHOt 'FORCE VECTOR', /,IH »iP3D12.5) 

0022 

0023 

611 

612 

FuRMATdH0f3X, 'PREDICTED STATE VEC TOR * /4X , 1P6012 - 5 ) 
FGRMATdHO, 3X, 'ERROR IN STATE VECTOR • /4X , IP6D12* 5 ) 

0024 

0026 

613 
6 14 

FORMAT! IHO, 3X, 'TRUE OBSE RVATI ON S • /4X , IP6012 . 5 ) 
format (IHO, 3X, ' PREDICTEO OBSERVATI ONS ' /4X , IP 601 2 . 5 ) 

57ZS 

0027 

6 15 
617 

FORMAT t IHO, 3Xr 'fitSlOUAL ERROR ' /4X , IP6D12 . 5) 
FORMAT! 1H0,3X, 'COVARIANCE MATRIX' ) 

0028 

0029 

6ie 

619 

FORMATdh ,6X, IP6D12.5I 

FORMAT! Ihl, lOX, ' SIMULATION RESULTS',////) 

0030 

0051 

6 20 

IF! L$Ld 1 )i^RITE 16,619) 

FORMAT! IH ,3X, 'NORMED POSITION ERROR = ' , IPl Dl 2. 5 , 5X , ' NORMED VELOC 

0032 

5ITT FRRUR = * , 1PID12.5) 
FT=0,000 

0033 

0034 


T1=0.0D0 

T2=0.00U 

0035 
00 36 


00 1 1=1,3 
F! I )=F( I )=}‘CF1 

00 37 
0038 


PT=PTFF( I 
T1=T1+XT( I )*XT( I ) 

”^39 

0040 

1 

t2=t2 + XT! I + 3)*XT( I 'fTI . - 

FT= DSQRT ( FT ) 

0041 

0042 


X S= OSQRtTtl r 
XV=DSgRT!T2> 

0044 


1F(1>L!9J)GU 10 2 
IF! XS.GT,XO)GU TO 3 

0045 

0046 


xu=xs 

TXS=TIME 

0047 

0048 

3 

IF! XV.GT,XVO)GU TO 2 
XVD=XV 

00 50 

2 

TXV = T IME ^ 

WR1T£!6,603)TIMF 

0051 

0052 


wRI t£! 6,604j XS,XV,FT ~ - ■ - 

CHK=OABSI TF-TIME) 

00 5 3 
0054 


IF! CHK.LT.OT ) TF=TF+DELT ' ' 

IF! LiL! 2) )RETURN 
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G LEVEL 

21 CYCLOT 

0055 


DO 99901 I$=1*ISLEN 

005t> 

99901 

Xt( U»=XTU$ I-XPI1$) 

0057 


PNQRMsOSORU XE 1 1 )**2'»-xe 12)**2+X£ ( 3>*=«'2 I 

0O5S 


VNJRM = OSORT( XE ( 4)*«=2<-XE < 5J **2+XE < 6 » +*2 1 

0059 


TE (6»610)F 

0060 


WKI Tt(6,60l)XT 



WkITE(6,6il)XP 

0062 


WRIT£(6»612)Xc 

JO 63 


k-PI Tc( 6,620)PNiGRM,VNGRM 

0064 


WRI Tcl 6, 613)ZT 

0065 


UKI TE< 6,614) ZP 

0066 


iJRlT£(6,6l5)Z£ 

0067 


tJRI Tt( 6,617 ) 

0068 


DO 6 1=1, I SLEN 

0069 

6 

WRITtt6,6iaMP( I , J) ,0 = 1 ,1 SLEN) 

0070 


T£(o,608) 

0)71 

9 9902 

TTlTTT^lT 

0072 


RETURN 

0073 


EKTr'.y TERM IN 

0074 


IF( XS.LT. 1. 0D-2.AND.XV.lt. 1.0D-41G0 TO 4 

0075 


IF{ TIHE.GE.TF)GO TO 5 

0076 


return 

00 7 7 

4 

WRI TE< 6, 605) 

0076 


L$L (6I=LF 

0079 


RETURN 

OOoO 

5 

WRI TE(o,609) 

0081 


LiL (6)=LF 

0082 


RETURN 

00 6 3 


ENTRY RECAP 

0064 


Wki TE(6,606)xn,TXS 

0085 


WRI TE(6,607)XVO,TXV 

0086 


RETURN 

0087 


ENTRY CYINir 

0086 


X0= 1.0D40 

008 9 


XV3 = l.OL)40 

0090 


CF1=1 .0D0/( 9. 806650-3*8. 64D4*<‘2) 

0091 


RETURN 

0002 


END 
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FORTRAN 

IV G LEVEL 

21 MINV 


0001 



subroutine MINVI AtNfNS0tLtM,8IGA) 


0002 



IMPLICIT REALMS (A-HtO-ZI 

^ - 

0003 

C 


DIMENSION AINSQ) tLI 6) tH(6) 



C 


description of PARAMETERS 



C 


A - INPUT MATRIX, DESTROYED IN COMPUTATION 

AND REPLACED BY 


C 


resultant inverse* 



C 


N - ORDER OF MATRIX A 



C 


aiGA - resultant DETERMINANT 



C 


L - WORK VECTOR OF LENGTH N 



c 

c 


M - WORK VECTOR OF LENGTH N 


0004 



NK=-N 


0005 



DO 190 K=l,N 


1)006 



NK=NK+N 


0007 



LU )=K 


0006 



M (< )=K 


0009 



KK=NK+K 


0010 



B1GA=A(KK) 


0011 



DC 30 J=K,N 


0012 



I Z=N»( J-1) 


0013 



DO 30 I=K,N 


0014 



IJ= IZ + I 


OJlb 


10 

1F( DABS! RIGA )-OABS(Al IJJ M 20,^0,50 


0016 


20 

hlGA = A( I J ) 


0017 



L(K )=1 


00 IB 



M(K )=J 


0019 


30 

CONTINUE 



c 





c 


U4TERCHANGE ROWS 


0020 

c 


J=L IK ) 


0021 



IF(J-K) 60,60,40 


0022 


40 

KI=K-N 


0023 



DO 50 I=1,N 


0024 



KI=Kl+N 


0025 



HOLD=-AIKI ) 


0026 



Jl=Kl-K+J 


0027 



AIK I )=AI JI J 


002a 


50 

A(JI) =HDLD 



r 

c 


intepxhangf columns 



c 




0029 


60 

I=M(K ) 


0o30 



IF( I-K) 90,90, 70 


00 31 


70 

JP=N#I I-i ) 


00 32 



DO 60 J=1,N 


0033 



JK=NK+J 


0034 



JI= JP+J 


00 35 



HOLO=-AIJK) 


0036 



A( JK)=AI JI ) 


0037 


80 

Aun =HOLO 



c 





c 


DIVIDE COLUMN BY MINUS PIVOT (VALUE OF PIVOT 

ELEMENT IS 


c 


CO NT A INFO IN BIGA) 



c 




0038 


90 

IFtBIGA) 110,100,110 


0039 


100 

RtTUf-N 



23 


FORTRAN IV G LcVEL 2l 


HINV 


0040 

0041 

110 

DO 130 I = UN 

IF( I~K) 120f 130tl20 

0042 

0043 

120 

IK=NK+I 

A{ 1K)=AI IK)/<-3IGAJ 

00^4 

130 

CONTINUE 


C 



c 

REDUCE MATRIX 


c 


0045 


CO 160 1=1, N 

0046 


IK=NK+I 



U=I-N 



DG 160 J=l,N 



IJ=IJ+N 

0050 


IF( I-KI 140,160,140 

0051 

140 

IF(J-K) 150,160,150 

00 52 

150 

KJ= IJ-I+K 

UTT53 


A(iJ) = A( IK.)*A(KJ)+A(IJI 

0054 

160 

CONTINUE 


c 

c 

DIVIDE ROW BY PIVOT 


c 


00 55 


KJ=K-N 



DO 160 J=1,N 

00 57 


K J=KJ+N 

00 56 


IF(J-K) 170,130,170 

0059 

170 

>*(<J)=A(KJ» / 6IGA 

0060 

180 

CONTINUE 


C 



— c 



c 


0061 


AI<K)=1.0/BIGA 

0062 

190 

CONTINUE 


c 



c 

FINAL kOiK and COLUMN INTERCHANGE 


c 




K=N 

0064 

2 00 

K=( K-IJ 

0065 


IFIKI 270,270,210 

0066 

210 

I=L(KI 



IF( l-KI 240,240,220 

51^6^ 


JO=N»(K-n 

0069 


JR = N<=( I-i 1 

goyj5 


00 230 J=l,N 

0071 


JK=JO+J 



HOlO^AUK ) 

0073 


JI= JR+J 

0074 


AUKI =-A( J 1 ) 

0075 

2 30 

AUI) =HOLO 

0C76 

240 

J=M(K) 



IFIJ-K) 200,200,250 

0078 

2 50 

KI=K-N 

0079 


CO 260 1=1, N 



KI=K1+N 

0061 


hold=aiki > 

0082 


JI=KI-K+J 

0063 


A(Kn=-A(JI) 

■ O084 

260 

aiju =hqld 

0065 


GO TO 200 


FORTRAN 

IV G LEVEL 

21 

MINV 




0086 

0087 

2 70 

return 

FND 






2k 






